#ifndef OBSTACLEAVOIDER_H_
#define OBSTACLEAVOIDER_H_

#include <std_msgs/Float32MultiArray.h>
#include <boost/thread.hpp>
namespace std {
using boost::mutex;
using boost::condition_variable;
using boost::unique_lock;
using boost::thread;
}

#include "../Driver.h"

#define MAX_RANGE_SIZE 720 // maximal size for laser scan data

class ObstacleAvoider {
private:

	// ROS management objects
	ros::NodeHandle m_node_handler;
	ros::Rate* m_loop_rate;
	ros::Publisher m_sensors_publisher;
	ros::Subscriber sub_sensors_fl;

	// utility objects
	Driver* m_driver; // object to handle movement topics
	double m_speed; // robot's speed
	std::mutex m_mutex; // used for synchronized access to data
	bool m_run_once; // indicates if terminates after bypassing first obstacle
	float m_right_bin; // average distance of right section of laser scan (see .cpp for indices)
	float m_center_bin; // average distance of central section of laser scan (see .cpp for indices)
	float m_left_bin; // average distance of left section of laser scan (see .cpp for indices)
	float m_range[MAX_RANGE_SIZE]; // current sample from front laser

	/**
	 * Subscribes to relevant sensors for the wall following
	 */
	void subscribeToSensor();

	float avgElements(const std_msgs::Float32MultiArray::ConstPtr& range, int start, int end);
	float avgElements(const float range[], int start, int end);
	float getMin(float x, float y, float z);
	void getBins(float& right, float& center, float& left);
	void setBins(float right, float center, float left);
	void getRange(float range[]);
	void setRange(const std_msgs::Float32MultiArray::ConstPtr& range);
	void bypassObstacle(float& turn_angle, float& obstacle_edge);
	short getSign(float x);
	float calcTurnAngle(int diff, float distToEdge);
	int calcMaintainTime(float obstacle_edge);
	bool findRightEdge(float range[], int& right_edge_index, float& right_edge);
	bool findLeftEdge(float range[], int& left_edge_index, float& left_edge);
	void fixOrientation();
	void closestWindow(float range[], float& avg_dist, short& sign);

public:

	ObstacleAvoider(Driver* driver, double speed, bool runOnce);

	virtual ~ObstacleAvoider();

	/*
	 * drives robot and avoids obstacles on the way
	 */
	void run();

	/*
	 * callback to get data from front laser
	 */
	void frontLaserHandler(const std_msgs::Float32MultiArray::ConstPtr& range);

	bool checkObstacle(float min_right, float min_center, float min_left);
};

#endif /* OBSTACLEAVOIDER_H_ */
